# global r1 = 0
# global r2 = 0
# global c1 = 0
# global c2 = 0

# global a1 = 1/(r1*c1)+1/(r2*c1)
# global a0 = 1/(r1*r2*c1*c2)

# function H_func(s)
#     result = a0/(s*s+a1*s+a0)
#     return result
# end

using ControlSystems
using XLSX
using CSV
using DataFrames

# 假设采样频率 fs
fs = 50  # 例如 1000 Hz
T = 1.0 / fs

# 3.84Hz低通滤波
r1 = 33000
r2 = 33000
c1 = 0.07e-6
c2 = 0.03e-6

a1 = 1/(r1*c1)+1/(r2*c1)
a0 = 1/(r1*r2*c1*c2)

# 定义连续时间传递函数
num = [a0]
den = [1, a1, a0]
sys = tf(num, den)
ssys = ss(sys)

sysd = c2d(ssys, T, :tustin)  # Tustin 方法是双线性变换

# 读取 Excel 文件中的数据
filename = "sample_data/20241202采样.csv"
data = DataFrame(CSV.File(filename))

# 提取各列数据
# point_numbers = data[:, 1]
# timestamps = data[:, 2]
# temperatures = data[:, 3]
# measurements = data[:, 4]
measurements = data[:, 2]
if @isdefined measurements
    println("measurements 已经定义")
else
    error("无measurement数据，后续无法进行")
end

if @isdefined origin_data
    origin_data = measurements
    println("局部变量 origin_data 已经定义，进行修改")
else
    origin_data = measurements
    println("局部变量 origin_data 未定义，进行定义")
end

if @isdefined point_numbers
    point_numbers = 1:length(measurements)
    println("局部变量 point_numbers 已经定义，进行修改")
else
    point_numbers = 1:length(measurements)
    println("局部变量 point_numbers 未定义，进行定义")
end

if @isdefined timestamps
    println("局部变量 timestamps 已经定义")
else
    timestamps = point_numbers
    println("局部变量 timestamps 未定义，进行定义")
end

if @isdefined temperatures
    println("局部变量 temperatures 已经定义")
else
    temperatures = point_numbers
    println("局部变量 temperatures 未定义，进行定义")
end

t = 0:T:(length(origin_data)-1)*T  # 时间向量
t = convert(Vector{Float64}, t)
# 模拟系统的响应
function get_origin_data(_,time)
    index = time*fs+1
    index = floor(index)
    index = convert(Int,index)
    return origin_data[index]
end

y, t, x = lsim(sysd, get_origin_data, t)
y = vec(y)
active_lp_filtered=y
# 绘制结果
using Plots
# plot(t[100:end], origin_data[100:end], label="Original Data", linewidth=2, color=:blue)
# plot!(t[100:end], y[100:end], label="Filtered Data", linewidth=2, color=:red, legend=:topright)
plot(t[6000:7000], origin_data[6000:7000], label="Original Data", linewidth=1, color=:blue)
plot!(t[6000:7000], y[6000:7000], label="Filtered Data", linewidth=1, color=:red, legend=:topright)
xlabel!("Time (s)")
ylabel!("Amplitude")
title!("Filter Response")
savefig("result/Whole_Sys/Active_LP_Filter_Response.png")
savefig("result/Whole_Sys/Active_LP_Filter_Response.svg")
println("Active_Filter_Response.png saved.")

LP_residuals = active_lp_filtered-measurements

plot(t[6000:7000], LP_residuals[6000:7000], label="Filtered Data", linewidth=2, color=:red, legend=:topright)
xlabel!("Time (s)")
ylabel!("Amplitude")
title!("LP Filter residuals")
savefig("result/Whole_Sys/Active_LP_Filter_residuals.png")
println("Active_Filter_Response.png saved.")

function kalman_filter_step(x,P,z)
    # function kalman_filter_step(A,B,H,Q,R,x,P)
    dt = 0.02  # 时间步长
    A = 1.0   # 状态转移矩阵
    B = 0.0   # 控制输入矩阵
    H = 1.0   # 观测矩阵
    Q = 0.01  # 过程噪声协方差
    # R = 4.272e-7   # 测量噪声协方差
    R = 4.272   # 测量噪声协方差
    # R = 1   # 测量噪声协方差
    
    x_hat = x
    x_hat_minus = A * x_hat + B * 0
    P_minus = A * P * A' + Q
    
    # 更新步骤
    K = P_minus * H' / (H * P_minus * H' + R)
    x_hat = x_hat_minus + K * (z - H * x_hat_minus)
    P = (1 - K * H) * P_minus
    
    return x_hat_minus,x_hat,P
end

function kalman_filter(z)
    x_hat_minus = 2.63394896
    x_hat = 2.63394896  # 初始状态估计
    P = 1.0      # 初始误差协方差
    
    filtered_values = zeros(length(z))
    
    for k in 1:length(z)
        # 预测步骤
        x_hat_minus,x_hat,P = kalman_filter_step(x_hat,P,z[k])
        filtered_values[k] = x_hat
    end
    
    return filtered_values
end

kalman_filtered = kalman_filter(active_lp_filtered)

plot(t[6000:7000], origin_data[6000:7000], label="Original Data", linewidth=1, color=:blue)
plot!(t[6000:7000], kalman_filtered[6000+5:7000+5], label="KF Filtered Data", linewidth=1, color=:red, legend=:topright)
xlabel!("Time (s)")
ylabel!("Amplitude")
title!("Filter Response")
savefig("result/Whole_Sys/KF_Filter_Response.png")
savefig("result/Whole_Sys/KF_Filter_Response.svg")
println("KF_Filter_Response.png saved.")

KF_Origin_residuals = kalman_filtered-measurements
kf_Filter_residuals = kalman_filtered-active_lp_filtered

plot(t[6000:7000], KF_Origin_residuals[6000:7000], label="KF Origin residuals", linewidth=2, color=:red, legend=:topright)
plot!(t[6000:7000], kf_Filter_residuals[6000:7000], label="KF LP residuals", linewidth=2, color=:blue)
xlabel!("Time (s)")
ylabel!("Amplitude")
title!("LP Filter residuals")
savefig("result/Whole_Sys/kf_Filter_residuals.png")
println("kf_Filter_residuals.png saved.")


Whole_Sys_data = DataFrame(
    Point_Number=point_numbers,
    Timestamp=timestamps,
    Temperature=temperatures,
    Original_Measurement=origin_data,
    Active_LP_Filtered=active_lp_filtered,
    KF_Filter_residuals = kf_Filter_residuals,
)

output_filename = "result/Whole_Sys/Whole_Sys.xlsx"

if isfile(output_filename)
    println("文件 $output_filename 已存在，将删除并重新保存。")
    rm(output_filename)  # 删除现有文件
else
    println("文件 $output_filename 不存在，将直接保存。")
end

XLSX.writetable(output_filename, Whole_Sys_data)

println("滤波后的数据已保存到 $output_filename")
output_filename = "result/Active_Filter/Whole_Sys.csv"

if isfile(output_filename)
    println("文件 $output_filename 已存在，将删除并重新保存。")
    rm(output_filename)  # 删除现有文件
else
    println("文件 $output_filename 不存在，将直接保存。")
end

CSV.write(output_filename, Whole_Sys_data)
